#ifndef DRIVE_SUBSYSTEM_H
#define DRIVE_SUBSYSTEM_H

#include "WPILib.h"
#include "RobotSubsystem.h"
#include "OperatorButton.h"

class EntechDriveSubsystem : public RobotSubsystem
{
public:
	EntechDriveSubsystem();
	virtual ~EntechDriveSubsystem();
	
	virtual void DoRobotInit(void);
	virtual void DoAutonomousInit(void);
	virtual void DoTeleopInit(void);
	virtual void DoDisabledInit(void);
	virtual void DoAutonomousPeriodic(void);
	virtual void DoTeleopPeriodic(void);
	virtual void DoDisabledPeriodic(void);

	void DriveByDistanceInInches(double leftInches, double rightInches);
	bool IsAutoDriveFinished();
private:
	void EntechDrive(Joystick *js_left, Joystick *js_right);
	void SetDriveMotors(double left, double right);
	void DriveByDistance(double leftEncoderTarget, double rightEncoderTarget);
	//void ApplyDeadzone(double xAxis, double yAxis);
	
	Joystick *m_leftJoystick;
	Joystick *m_rightJoystick;
	
	Talon *m_leftDriveMotor;
	Talon *m_rightDriveMotor;
	
	Encoder *m_leftEncoder;
	Encoder *m_rightEncoder;
	
	OperatorButton *m_driveModeSwitch;
	OperatorButton *m_automaticEncoderDrive;
	
	bool m_driveMode;
	bool m_autoDriveLock;
	bool m_leftFinished;
	bool m_rightFinished;
};

#endif
